/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;

/**
 *
 * @author bryanfullam
 */
public class DriveEncoder {
    static Encoder leftEncoder,rightEncoder;
    Timer timer = new Timer();
   
    
     public DriveEncoder(int port1,int port2,int port3,int port4)
    {
        leftEncoder = new Encoder(port1,port2);
        leftEncoder.setDistancePerPulse(1.0);
        rightEncoder = new Encoder(port3,port4);
        rightEncoder.setDistancePerPulse(1.0);
    }
    
    /*public DriveEncoder(int port1, int port2)
    {
        myEncoder = new Encoder(port1, port2);
        myEncoder.setDistancePerPulse(1.0);
    }
    */
    public void startEncoder()
    {
        leftEncoder.reset();
        leftEncoder.start();
        rightEncoder.reset();
        rightEncoder.start();
    }
    
    public static double getLeftDistance()
    {
        return leftEncoder.getDistance();
       
    }
    public static double getRightDistance()
    {
        return rightEncoder.getDistance();
       
    }
    
    public static void stopEncoders()
    {
        leftEncoder.stop();
        rightEncoder.stop();
    }
    
    public static void resetEncoders()
    {
        leftEncoder.reset();
        rightEncoder.reset();
    }
    
    /*public void driveDistanceBody(int distance)
    {
       this.startEncoder();
       timer.stop();
       timer.reset();
       timer.start();
       while (this.getDistance() < distance)
       {
           MecanumDrive.driveBodyFrameStatic(1, 0, 0);
           if (timer.get() > 5)
           {
               break;
           }
       }
       MecanumDrive.driveBodyFrameStatic(0, 0, 0);
    }
    
    public void driveDistanceField(int distance)
    {
       this.startEncoder();
       timer.stop();
       timer.reset();
       timer.start();
       while (this.getDistance() < distance)
       {
           MecanumDrive.driveFieldFrameStatic(1, 0, 0);
           if (timer.get() > 5)
           {
               break;
           }
       }
    }*/
    
    
}
